from time import sleep
import numpy as np
import rospy
from unitree_legged_msgs.msg import LowCmd, MotorCmd


class LowCmdPublisher():
    def __init__(self, rname):
        self.robot_name = rname
        self.servo_pub = list(np.arange(12))
        self.servo_pub[0] = rospy.Publisher("/" + self.robot_name + "_gazebo/FR_hip_controller/command", MotorCmd, queue_size=1)
        self.servo_pub[1] = rospy.Publisher("/" + self.robot_name + "_gazebo/FR_thigh_controller/command", MotorCmd, queue_size=1)
        self.servo_pub[2] = rospy.Publisher("/" + self.robot_name + "_gazebo/FR_calf_controller/command", MotorCmd, queue_size=1)
        self.servo_pub[3] = rospy.Publisher("/" + self.robot_name + "_gazebo/FL_hip_controller/command", MotorCmd, queue_size=1)
        self.servo_pub[4] = rospy.Publisher("/" + self.robot_name + "_gazebo/FL_thigh_controller/command", MotorCmd, queue_size=1)
        self.servo_pub[5] = rospy.Publisher("/" + self.robot_name + "_gazebo/FL_calf_controller/command", MotorCmd, queue_size=1)
        self.servo_pub[6] = rospy.Publisher("/" + self.robot_name + "_gazebo/RR_hip_controller/command", MotorCmd, queue_size=1)
        self.servo_pub[7] = rospy.Publisher("/" + self.robot_name + "_gazebo/RR_thigh_controller/command", MotorCmd, queue_size=1)
        self.servo_pub[8] = rospy.Publisher("/" + self.robot_name + "_gazebo/RR_calf_controller/command", MotorCmd, queue_size=1)
        self.servo_pub[9] = rospy.Publisher("/" + self.robot_name + "_gazebo/RL_hip_controller/command", MotorCmd, queue_size=1)
        self.servo_pub[10] = rospy.Publisher("/" + self.robot_name + "_gazebo/RL_thigh_controller/command", MotorCmd, queue_size=1)
        self.servo_pub[11] = rospy.Publisher("/" + self.robot_name + "_gazebo/RL_calf_controller/command", MotorCmd, queue_size=1)

    def sendServoCmd(self, lowCmd:LowCmd):
        for m in range(12):
            self.servo_pub[m].publish(lowCmd.motorCmd[m])
        sleep(0.001)